/**
* @copyright Copyright (c) 2022-2023 Grab GEO IoT Team
* @file fsmc.c
* @author wenzhu.tu
* @version 0.1
* @date 2023-03-23
* @brief fsmc
*/

/******************************* Includes ********************************/
#include "fsmc.h"


/******************************* Macros ******************************/


/******************************* Typedef ******************************/

/******************************* Variables ******************************/

/******************************* Functions ******************************/

static void list_del(fsmc_handle_t *head, fsmc_handle_t *node)
{
    if(head == NULL || node == NULL)
    {
        FSMC_E("head or node is null");
        return;
    }

    if(head == node)
    {
        FSMC_E("head is not allow delete");
        return;
    }

    fsmc_handle_t *cur = head->next;
    while(cur != head)
    {
        if(cur == node)
        {
            fsmc_handle_t *last_node = cur->last;
            fsmc_handle_t *next_node = cur->next;
            last_node->next = next_node;
            next_node->last = last_node;
            free(cur);
            break;
        }
        cur = cur->next;
    } 
}

static void list_add(fsmc_handle_t *head, fsmc_handle_t *node)
{
    if(head == NULL || node == NULL)
    {
        FSMC_E("head or node is null");
        return;
    }
    
    fsmc_handle_t *last_node = head->last;
    fsmc_handle_t *next_node = head;
    node->last = last_node;
    last_node->next = node;
    next_node->last = node;
    node->next = next_node;

}

static int64_t get_time_ms()
{
    struct timespec ts = {.tv_sec=0,.tv_nsec=0}; 
    clock_gettime(CLOCK_MONOTONIC, &ts);
    int64_t ms = ts.tv_sec * 1000 * 1000 +  ts.tv_nsec / 1000;
    return ms;
}

static void *fsmc_process(void *usr)
{
     FSMC_D("FSMC process entry");
     fsmc_handle_t *handle = (fsmc_handle_t *)usr;
     while(!handle->exit_flg)
     {
         pthread_mutex_lock(&handle->mutex);
         if(handle->cur_status != NULL && handle->cur_status->run != NULL && handle->cur_status->loop_run)
         {
            int64_t ms = get_time_ms();
            handle->cur_status->run(ms);
         }
         pthread_mutex_unlock(&handle->mutex);
         usleep(SCHEDULE_DELAY);
     }
     pthread_mutex_destroy(&handle->mutex);
     free(handle);
     FSMC_D("FSMC process exit");
     return NULL;
}

fsmc_handle_t *fsmc_init()
{
   fsmc_handle_t *handle = (fsmc_handle_t *)malloc(sizeof(fsmc_handle_t));
   if(handle == NULL)
   {
      FSMC_E("handle malloc fail");
      return NULL;
   }

   handle->enter = NULL;
   handle->run = NULL;
   handle->exit = NULL;
   handle->last = handle;
   handle->next = handle;
   handle->cur_status = handle;
   pthread_mutex_init(&handle->mutex, NULL);

   pthread_t trd;
   if(pthread_create(&trd, NULL, fsmc_process, (void *)handle) == 0)
      pthread_detach(trd);
   return handle;
}

void fsmc_deinit(fsmc_handle_t **handle)
{
    fsmc_handle_t *_handle = *handle;
    if(_handle == NULL)
    {
       FSMC_E("_handle is null");
       return;
    }
    
    pthread_mutex_lock(&_handle->mutex);
    fsmc_handle_t *temp = _handle->next;
    while(temp != _handle)
    {
        fsmc_handle_t *next = temp->next;
        list_del(_handle, temp);
        temp = next;
    }
    _handle->exit_flg = true;
    pthread_mutex_unlock(&_handle->mutex);
    *handle = NULL;
}


int  fsmc_add(fsmc_handle_t *handle, fsmc_status_func_t *node)
{
    if(handle == NULL)
    {
       FSMC_E("handle is null");
       return -1;
    }

    fsmc_handle_t *fsmc_node = (fsmc_handle_t *)malloc(sizeof(fsmc_handle_t));
    if(fsmc_node == NULL)
    {
       FSMC_E("fsmc_node malloc fail");
       return -1;
    }
   
    fsmc_node->last = NULL;
    fsmc_node->next = NULL;
    fsmc_node->cur_status = NULL;
    fsmc_node->enter = node->enter;
    fsmc_node->run = node->run;
    fsmc_node->exit = node->exit;
    fsmc_node->loop_run = node->loop_run;

    pthread_mutex_lock(&handle->mutex);
    list_add(handle, fsmc_node);
    pthread_mutex_unlock(&handle->mutex);

    return 0;
}

int fsmc_action(fsmc_handle_t *handle, FSMC_ACTION_e action)
{
     if(handle == NULL)
     {
        FSMC_E("handle is null");
        return -1;
     }

     pthread_mutex_lock(&handle->mutex);
     if(action == FSMC_ACTION_INIT)
     {
        if(handle->cur_status->exit)
        {
           handle->cur_status->exit(get_time_ms()); 
        }

        handle->cur_status = handle->next;

        if(handle->cur_status->enter)
        {
           handle->cur_status->enter(get_time_ms()); 
        }

        if(!handle->cur_status->loop_run && handle->cur_status->run)
        {
           handle->cur_status->run(get_time_ms());  
        }
     }
     else if(action == FSMC_ACTION_LAST)
     {
        if(handle->cur_status->last == handle)
        {
            FSMC_E("last node is head, change fail");
            pthread_mutex_unlock(&handle->mutex);
            return -1;
        }

        if(handle->cur_status->exit)
        {
           handle->cur_status->exit(get_time_ms()); 
        }

        handle->cur_status = handle->cur_status->last;

        if(handle->cur_status->enter)
        {
           handle->cur_status->enter(get_time_ms()); 
        }

        if(!handle->cur_status->loop_run && handle->cur_status->run)
        {
           handle->cur_status->run(get_time_ms());  
        }
     }
     else if(action == FSMC_ACTION_NEXT)
     {
        if(handle->cur_status->exit)
        {
           handle->cur_status->exit(get_time_ms()); 
        }

        handle->cur_status = handle->cur_status->next;

        if(handle->cur_status == handle)
        {
            handle->cur_status = handle->next;
        }

        if(handle->cur_status->enter)
        {
           handle->cur_status->enter(get_time_ms()); 
        }

        if(!handle->cur_status->loop_run && handle->cur_status->run)
        {
           handle->cur_status->run(get_time_ms());  
        }
     }
     pthread_mutex_unlock(&handle->mutex);
     return 0;
}


void event_enter(int64_t time_ms)
{
     FSMC_D("enter time_ms: %ld", time_ms);
}

void event_run(int64_t time_ms)
{
     FSMC_D("run time_ms: %ld", time_ms);
}

void event_exit(int64_t time_ms)
{
     FSMC_D("exit time_ms: %ld", time_ms);
}

static void *task_test(void *usr)
{
    fsmc_handle_t *handle = fsmc_init();
    if(handle == NULL)
    {
        return NULL;
    }

    fsmc_status_func_t status_func = {true, event_enter, event_run, event_exit};
    fsmc_add(handle, &status_func);
    fsmc_add(handle, &status_func);
    fsmc_add(handle, &status_func);
    fsmc_add(handle, &status_func);
    fsmc_action(handle, FSMC_ACTION_INIT);
    while(1)
    {
        fsmc_action(handle, FSMC_ACTION_NEXT);
        sleep(2);
    }

    return NULL;
}

void fsmc_test()
{
    pthread_t trd;
    pthread_create(&trd, NULL, task_test, NULL);
    pthread_detach(trd);
}
/******************************* End of file ******************************/